/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of SpeedCtrl interface Server.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include "SpeedCtrlServer.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::SpeedCtrl;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CSpeedCtrlServer::CSpeedCtrlServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                                   const int aInterfaceMinor,
                                   const std::string &aDescription)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_SPEEDCTRL, aInterfaceMinor),
    CSync(1,1),
    CThread(2),
    iMaxCommandInterval_ms(100),
    iMinCommandInterval_ms(10),
    iCommandTimeoutTime_ms(500),
    iClientCommanding(false),
    iMaxSpeeds(),
    iMinSpeeds(),
    iSpeedCtrlCommandArray(),
    iServiceDescription(aDescription)

{
}
//*****************************************************************************

CSpeedCtrlServer::~CSpeedCtrlServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!IsActive()) {
    // Register service.
    //RegisterProvidedService("MaCI - SpeedCtrl Interface");
    RegisterProvidedService("MaCI-SpeedCtrlServer-Speed; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-SpeedCtrlServer-Cmd; '" + iServiceDescription + "'");
    
    // Start the handler Thread
    SetActive(true);
    CThread::RunThread();

    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed SpeedCtrlServer instance %p.", this);
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (IsActive()) {
    // Unregister service
    UnregisterProvidedService();
    UnregisterAcceptedService();

    // Stop the handler Thread
    SetActive(false);
    /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread();
    
    
    dPrint(4,"Succesfully Close():ed SpeedCtrlServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlServer::WaitCommand(CSpeedCtrlData &data, 
                                               const unsigned int aTimeout_ms,
                                               bool aSkipQueued)
{
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  int timeout = aTimeout_ms;

  data.Reset();

  Lock();
  do {
    const unsigned int asize = iSpeedCtrlCommandArray.size();
    // Check whether there are commands in buffer.
    if (asize > 0) {
      // Got element. So; take reference of its pointer, copy it to
      // 'data', delete it. Pop the element from queue.
      CSpeedCtrlData *d;
      
      // Check whether we want to skip queue 
      if (aSkipQueued && asize > 1) {
        // This is only executed if there is >1 in queue.  Otherwise
        // default branch is executed.  Want to skip, so get back()
        // reference.
        d = iSpeedCtrlCommandArray.back();
        // Copy data
        data = *d;

        // Delete all elements.
        for(EACH_IN_i(iSpeedCtrlCommandArray)) {
          delete *i;
        }
        // Clear array.
        iSpeedCtrlCommandArray.clear();

        // print debug
        dPrint(4,"Flushed %d unprocessed commands out of buffer",
               asize-1);

      } else {
        // No skipping, so get front() reference;
        d = iSpeedCtrlCommandArray.front();
        // Copy data.
        data = *d;
        // Delete source
        delete d;
        // pop the entry from queue.
        iSpeedCtrlCommandArray.pop_front();

      }

      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
    
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(begin);
  } while(timeout > 0 && IsActive());
  Unlock();

  // Change result type if timeout was the reason (Depends that
  // timeout is zero or equal, and the result was not previously set
  // to OK)
  if (timeout <= 0 && result == KMaCIError) result = KMaCITimeout;


  return result;
}
//*****************************************************************************

void CSpeedCtrlServer::ClearCommandBuffer(void)
{
  Lock();
  dPrint(8,"Clearing commandbuffer, got %d entries", iSpeedCtrlCommandArray.size());
  iSpeedCtrlCommandArray.clear();
  Unlock();
}
//*****************************************************************************

bool CSpeedCtrlServer::SendSpeedEvent(const CSpeedCtrlData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);
  aData.Print(10);
  const TSpeed *p = aData.GetSpeed();
  if (p) {
    EMaCIError e = 
      SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                     aData.GetBinBagContainerPtr()->GetBufferSize());
 
    result = (e == KMaCIOK);

  } else {
    dPrint(4,"No TSpeed element in container, will not send!");

  }
  
  return result;



}
//*****************************************************************************

void CSpeedCtrlServer::SetCommandSendingIntervalsLimits(unsigned int aMaxCommandInterval_ms,
                                                        unsigned int aMinCommandInterval_ms)
{
  Lock();
  iMaxCommandInterval_ms = aMaxCommandInterval_ms;
  iMinCommandInterval_ms = aMinCommandInterval_ms;

  Unlock();

}
//*****************************************************************************

void CSpeedCtrlServer::SetSpeedLimits(float aMaxSpeed, 
                                      float aMaxAngularSpeed, 
                                      float aMaxAcceleration,
                                      float aMinSpeed, 
                                      float aMinAngularSpeed, 
                                      float aMinAcceleration)
{

  Lock();
  iMaxSpeeds.speed = aMaxSpeed;
  iMaxSpeeds.angularSpeed = aMaxAngularSpeed;
  iMaxSpeeds.acceleration = aMaxAcceleration;
  iMinSpeeds.speed = aMinSpeed;
  iMinSpeeds.angularSpeed = aMinAngularSpeed;
  iMinSpeeds.acceleration = aMinAcceleration;
  Unlock();


}
//*****************************************************************************

void CSpeedCtrlServer::SetCommandTimeoutTime(unsigned int aCommandTimeoutTime_ms)
{
  Lock();
  iCommandTimeoutTime_ms = aCommandTimeoutTime_ms;
  Unlock();

}
//*****************************************************************************
//*****************************************************************************

int CSpeedCtrlServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CSpeedCtrlData d;
  bool r;

  // Before even attempting to decode, read access.
  MaCICtrl::TAccess a;
  CMaCI::GetMaCICtrlServer()->GetAccessInfo(a, 
                                            iInterfaceMajor,
                                            iInterfaceMinor,
                                            msg.getSenderId(),
                                            true);
  // Test for access.
  if (a.IsAccess()) {
    dPrint(ODTEST,"Access granted for nodeID: %08X, proceed with command.",
           msg.getSenderId());

    r = bb.Decode(msg.getData(), msg.getDatalength());
    if (r) {
      // Succesfully decoded as BinBag.
      r = d.DecodeFrom(&bb);
      if (r) {
        // Succesfully decoded as SpeedCtrl
        // Now, Check whether it was Command, if so, process it and construct reply
        gim::binbag::CBinBag replybb;
        CSpeedCtrlData reply;
        bool send_reply = false;

        // Do encoding to this binbag
        reply.EncodeTo(&replybb);
      
        const TCommand *cmd = d.GetCommand();
        if (cmd) {
          switch(cmd->cmd) {
          
            // Set Commands: by default passed to user
          case KCommandSetStop:
          case KCommandSetSpeed: {
            // Default action for Set commands is to save them to CommandQueue
          
            // Debug
            dPrint(10, "Got Command to be passed to User (Command was %d)",
                   cmd->cmd);
          
            // Create new dynamic entry
            CSpeedCtrlData *new_entry = new CSpeedCtrlData(d);

            // Lock, Push, Notify, Unlock
            Lock();
            iClientCommanding = true;
            iSpeedCtrlCommandArray.push_back(new_entry);
            Notify();
            Unlock();
            break;
          }
          case KCommandGetCommandIntervals:{
            dPrint(ODTEST,"GET COMMAND INTERVALS!");
            Lock();
            TCommandIntervals intervals(iMaxCommandInterval_ms,
                                        iMinCommandInterval_ms);
            reply.SetCommand(KReplyGetCommandIntervals);
            reply.SetCommandIntervals(intervals);

            send_reply = true;
            Unlock();
            break;
          }

          case KCommandGetSpeedLimits:{

            Lock();
            reply.SetCommand(KReplyGetSpeedLimits);
            reply.SetSpeedLimits(iMaxSpeeds,iMinSpeeds);
            send_reply = true;
            Unlock();
            break;
          };

          case KCommandGetTimeoutTime:{
            Lock();
            reply.SetTimeoutTime(TTimeoutTime(iCommandTimeoutTime_ms));
            Unlock();
            dPrint(ODDEBUG(1),"Sending Reply to %x, msgid: %u",  msg.getSenderId(), msg.messageId);
            result = SendSynchronousMessageReply(reply.GetBinBagContainerPtr()->GetBuffer(),
                                                 reply.GetBinBagContainerPtr()->GetBufferSize(),
                                                 msg.getSenderId(),
                                                 iInterfaceMajor,
                                                 iInterfaceMinor,
                                                 msg.messageId);
            break;
          }
            // case KCommandGetJotain: {
            // }

          default:
            dPrint(4,"Unidentified command! %d (%x)! Ignore.",
                   cmd->cmd, cmd->cmd);
            break;
          }
        }
      
        // Check whether reply needs to be sent
        if (send_reply) {
          dPrint(ODTEST, "Sending reply TO 0x%x", msg.getSenderId());
          reply.Print(10);
          int gr = iGIMIPtr->send(msg.getSenderId(),
                                  replybb.GetBuffer(),
                                  replybb.GetBufferSize(),
                                  iInterfaceMajor,
                                  iInterfaceMinor,
                                  msg.messageId); // Reply with same ID
          if (gr == GIMI_OK) {
            dPrint(ODTEST,"Succesfully sent message with %d BinBag elements to '%p'",
                   replybb.GetElementCount(),
                   msg.getSenderId());

          } else {
	  
            dPrint(ODTEST,"Failed to send message with %d BinBag elements to '%p'",
                   replybb.GetElementCount(),
                   msg.getSenderId());
          }
        }
      }
    }
  } else {
    dPrint(ODSTATUS,"Access denied for nodeID: %08X, will not execute command.",
           msg.getSenderId());

  }  
  return result;
}
//*****************************************************************************

int CSpeedCtrlServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  MaCI::Common::TTimestamp lastTStamp;
  // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    if(!iClientCommanding){
      e = ReceiveInterfaceMessage(gmsg, 1000);
    }else{
      e = ReceiveInterfaceMessage(gmsg, iCommandTimeoutTime_ms);
    }

    if (e == KMaCIOK) {
      lastTStamp.SetToCurrentTime();
      dPrint(8,"Got GIMIMessage %d - Processing.", gmsg.messageId);
      ProcessMessage(gmsg);
      
    } else if(e == KMaCITimeout && iClientCommanding){
      //Client is commanding but no message within timeout time. 
      //Construct SpeedCtrlData with KCommandTimeout-command to alert
      //the module
      MaCI::Common::TTimestamp timenow;
      if(timenow.GetGimTime()-lastTStamp.GetGimTime()> iCommandTimeoutTime_ms){
        dPrint(5,"No message from client within timeout time(%d ms). Creating KCommandTimeout-message to module",
               iCommandTimeoutTime_ms);
        // Create new dynamic entry
        gim::binbag::CBinBag *bb = new gim::binbag::CBinBag();
        CSpeedCtrlData *new_entry = new CSpeedCtrlData();
        new_entry->EncodeTo(bb);
        new_entry->SetCommand(TCommand(KCommandTimeout));
         
        // Lock, Push, Notify, Unlock
        Lock();
        iSpeedCtrlCommandArray.push_back(new_entry);
        iClientCommanding = false;
        Notify();
        Unlock();
      }
    
     

    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      dPrint(1,"Received error from ReceiveInterfaceMessage! Abort!");
      break;
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
